from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import Pose
from math import pi

# 角度转弧度
DE2RA = pi / 180

class RPYPoser:
    def __init__(self,x):
        self.x = x[0]
        self.y = x[1]
        self.z = x[2]
        self.roll = x[3]
        self.pitch = x[4]
        self.yaw = x[5]
    def RPYToSIYUAN(self):
        pos = Pose()
        q = quaternion_from_euler(self.roll * DE2RA, self.pitch * DE2RA, self.yaw * DE2RA)
        pos.position.x = self.x
        pos.position.y = self.y
        pos.position.z = self.z
        pos.orientation.x = q[0]
        pos.orientation.y = q[1]
        pos.orientation.z = q[2]
        pos.orientation.w = q[3]
        # print(pos)
        return pos
<<<<<<< HEAD

class SIYUANPoser:
    def __init__(self,x):
        self.x = x[0]
        self.y = x[1]
        self.z = x[2]
        self.a = x[3]
        self.b = x[4]
        self.c = x[5]
        self.d = x[6]
    def SIYUANtoPose(self):
        pos = Pose()
        pos.position.x = self.x
        pos.position.y = self.y
        pos.position.z = self.z
        pos.orientation.x = self.a
        pos.orientation.y = self.b
        pos.orientation.z = self.c
        pos.orientation.w = self.d
        # print(pos)
        return pos
=======
        
class MSGPoser:
    def MSGToPose(x):
        pos = Pose()
        pos.position.x = x[0]
        pos.position.y = x[1]
        pos.position.z = x[2]
        pos.orientation.x = x[3]
        pos.orientation.y = x[4]
        pos.orientation.z = x[5]
        pos.orientation.w = x[6]
        # print(pos)
        return pos
        
>>>>>>> 7ab8b32aacdca90fbd07e836fad97c25c5aa0f47
